#参考URL -> https://qiita.com/tibigame/items/61cecf86fc978628bfee
#参考図書 -> ポールのロボット・マニピュレータ
import numpy as np
import sympy as sym
sym.init_printing()
Pi = sym.S.Pi # 円周率
#sympyの円周率の方を使うことをすすめる(こっちの方が量子化誤差が大きくなる.numpyも同様に大きい)
import math
pi = math.pi
# 角度変数
(J_1,J_2,J_3,J_4,J_5,J_6) = sym.symbols('J_1,J_2,J_3,J_4,J_5,J_6')
# リンクパラメータ
(a_1,a_2,a_3,d_4) = sym.symbols('a_1,a_2,a_3,d_4')
# リンクパラメータ
(j,a,d,alpha) = sym.symbols('j,a,d,alpha')
# T6
(n_x, n_y, n_z, o_x, o_y, o_z, a_x, a_y, a_z, p_x, p_y, p_z) = sym.symbols('n_x, n_y, n_z, o_x, o_y, o_z, a_x, a_y, a_z, p_x, p_y, p_z')
#sin.cosの簡易記述用
def S(a):
return sym.sin(a)
def C(a):
return sym.cos(a)
#回転・並進行列
def rotx(a):
return sym.Matrix([[1, 0, 0, 0], [0, C(a), -S(a), 0], [0, S(a), C(a), 0], [0, 0, 0, 1]])
def roty(a):
return sym.Matrix([[C(a), 0, S(a), 0], [0, 1, 0, 0], [-S(a), 0, C(a), 0], [0, 0, 0, 1]])
def rotz(a):
return sym.Matrix([[C(a), -S(a), 0, 0], [S(a), C(a), 0, 0], [0, 0, 1, 0], [0, 0, 0, 1]])
def trans(x, y, z):
return sym.Matrix([[1, 0, 0, x], [0, 1, 0, y], [0, 0, 1, z], [0, 0, 0, 1]])
# DH matrix
def DH(j, alpha, a, d):
return rotz(j)*trans(a,0,d)*rotx(alpha)
# inverse DH matrix
def DHi(j, alpha, a, d):
return rotx(-alpha)*trans(-a,0,-d)*rotz(-j)
# target項を式eqからくくり出すための関数
def obs(eq, target):
sol = sym.solve(eq, target)
for i in range(len(sol)):
display( sym.Eq(target, sol[i]))
print( str(len(sol)) +' equation(s) in total')
return sol
| 座標系 i | Z_i-1軸回りに角度θ_i | X_i軸周りにねじれ角α_iだけ回転 | 回転後のX_i-1 (=X_i)に沿って長さa_iだけ並進 | Z_i-1に沿って距離d_iだけ並進 | |
|---|---|---|---|---|---|
| 1 | $J_1$ | $\pi/2$ | $a_1$ | 0 | |
| 2 | $J_2+\pi/2$ | 0 | $a_2$ | 0 | |
| 3 | $J_3-J_2$ | $\pi/2$ | $a_3$ | 0 | |
| 4 | $J_4$ | $-\pi/2$ | 0 | $d_4$ | |
| 5 | $J_5$ | $\pi/2$ | 0 | 0 | |
| 6 | $J_6$ | 0 | 0 | 0 | $ |
easy=False
if(easy):
A1=sym.trigsimp( DH (J_1, Pi/2, 0, 0))
A3=sym.trigsimp( DH (J_3 - J_2, Pi/2, 0, 0))
A1i=sym.trigsimp( DHi (J_1, Pi/2, 0, 0))
A3i=sym.trigsimp( DHi (J_3 - J_2, Pi/2, 0, 0))
else:
A1=sym.trigsimp( DH (J_1, Pi/2, a_1, 0))
A3=sym.trigsimp( DH (J_3 - J_2, Pi/2, a_3, 0))
# inverse matrix
A1i=sym.trigsimp( DHi (J_1, Pi/2, a_1, 0))
A3i=sym.trigsimp( DHi (J_3 - J_2, Pi/2, a_3, 0))
A2=sym.trigsimp( DH (J_2+ Pi/2, 0, a_2, 0))
A4=sym.trigsimp( DH (J_4, -Pi/2, 0, d_4))
A5=sym.trigsimp( DH (J_5, Pi/2, 0, 0))
A6=sym.trigsimp( DH (J_6, 0, 0, 0))
# inverse matrix
A2i=sym.trigsimp( DHi (J_2+ Pi/2, 0, a_2, 0))
A4i=sym.trigsimp( DHi (J_4, -Pi/2, 0, d_4))
A5i=sym.trigsimp( DHi (J_5, Pi/2, 0, 0))
A6i=sym.trigsimp( DHi (J_6, 0, 0, 0))
A3
# 逆行列をかけると単位行列になることの確認
ret = A2i*A2
sym.trigsimp(ret)
T6=sym.Matrix([[n_x, o_x, a_x, p_x], [n_y, o_y, a_y, p_y], [n_z, o_z, a_z, p_z], [0, 0, 0, 1]])
T6
# forward kinematics
A56 = sym.trigsimp( A5*A6 )
A456 = sym.trigsimp( A4*A5*A6 )
A3456 = sym.trigsimp( A3*A4*A5*A6 )
A23456 = sym.trigsimp( A2*A3*A4*A5*A6 )
T = sym.trigsimp( A1*A2*A3*A4*A5*A6 )
T
# Masahiro Furukawa
# Aug, 17, 2020
#
# refernce : https://qiita.com/JmpM/items/4bea4997aaf406cca3b4
# Cソースを得る
for ii in range(4):
for jj in range(4):
idx = jj*4+ii
code = sym.ccode(T[idx],assign_to=('Trans['+str(jj)+']['+str(ii)+']'), standard='C89')
print(code)
print()
T16 = sym.trigsimp( A1i*T6 ) # eq(3.70)
T26 = sym.trigsimp( A2i*A1i*T6 ) # eq(3.71)
T36 = sym.trigsimp( A3i*A2i*A1i*T6 ) # eq(3.72)
T46 = sym.trigsimp( A4i*A3i*A2i*A1i*T6 ) # eq(3.73)
T56 = sym.trigsimp( A5i*A4i*A3i*A2i*A1i*T6 ) # eq(3.74)
# Left hand of (3.76)
A1iT6 = T16
A1iT6
# Right hand of (3.76)
A23456
より以下の等式群を得る
for idx in range(12):
display(sym.simplify ( sym.expand( sym.Eq( A1iT6[idx], A23456[idx])) ) )
# このうち位置に関する項のみを抽出すると
for idx in [3,7,11]:
display(sym.simplify ( sym.expand( sym.Eq( A1iT6[idx], A23456[idx])) ) )
# の3元連立方程式を得る. 上記第3式は,
sym.Eq(A1iT6[11] , A23456[11])
sol = sym.solve(p_y * C(J_1), J_1)
for i in range(len(sol)):
display( sym.simplify(sym.Eq(J_1, sol[i])))
print( str(len(sol)) +' equation(s) in total')
# しかし値域(-PI < J1 < PI)より , p_x = 0 なら直ちに
sol = sym.solve(p_y * C(J_1), J_1)
for i in range(len(sol)):
if -Pi < sol[0] and sol[i] < Pi:
display( sym.simplify(sym.Eq(J_1, sol[i])), 'where p_x = 0')
sol = sym.solve(p_x * S(J_1), J_1)
for i in range(len(sol)):
display( sym.simplify(sym.Eq(J_1, sol[i])))
print( str(len(sol)) +' equation(s) in total')
# であるが, 同様に値域(-PI < J1 < PI)より , p_y = 0 なら直ちに
sol = sym.solve(p_x * S(J_1), J_1)
for i in range(len(sol)):
if -Pi < sol[0] and sol[i] < Pi:
display( sym.simplify(sym.Eq(J_1, sol[i])) , 'where p_y = 0')
# 両辺をp_xで割ることができ,
# p_x != 0 ならば 前述の通りC(J1) != 0であることが必然的に求まるためC(J1)で両辺を割っていいため,
eq =sym.simplify((A1iT6[11] - A23456[11])/p_x/C(J_1))
display(eq)
# この方程式をJ1について解くと,
sol = sym.solve(eq, J_1)
for i in range(len(sol)):
display( sym.simplify(sym.Eq(J_1, sol[i])) , 'where p_x != 0')
sol_J1 = sol
がJ1に関する解析解である.
# 次に上記第2式に着目すると
sym.Eq(A1iT6[7] , A23456[7])
# であるから,上記第1式に S(J2) を代入すべく C(J2)を求めると
eq =A1iT6[7] - A23456[7]
target = C(J_2)
CJ2 = obs(eq, target)[0]
# 従って,S(J2)を求めるためには,C(J2)**2 + S(J2)**2 = 1 であるから標的となる式を再掲すると,
eq = sym.Eq(A1iT6[3] , A23456[3])
display(eq)
# を得る. J1が既知のため上式の左辺をLとおく
sub = A1iT6[3]
L = sym.symbols('L')
eq = eq.subs(sub, L)
display(sym.trigsimp(eq))
# S(J2)について整理すると,
target = S(J_2)
SJ2 = obs(eq, target)[0]
# ここでS(J2)**2 + C(J2)**2 = 1 より
eq = sym.trigsimp(sym.Eq((SJ2**2+CJ2**2),1))
display(eq)
display(sym.collect(sym.trigsimp(eq.expand()), [C(J_3),S(J_3)]))
# 方程式をJ3について解くと,
target = J_3
sol_J3 = obs(eq, target)
# Masahiro Furukawa
# Aug, 21, 2020
#
# refernce : https://qiita.com/JmpM/items/4bea4997aaf406cca3b4
# J3 に対するCソースを得る
code = sym.ccode(sub, assign_to=('L'), standard='C89')
print("// constant L \n" + code + "\n")
for ii in range(len(sol_J3)):
code = sym.ccode(sol_J3[ii], assign_to=('J3'), standard='C89')
print("// Solusion #"+ str(ii) +"\n" + code + "\n")
# 次に,J2について解く.C(J2)について,CJ2として得られているからこれらの方程式から以下を得る.
eq = sym.Eq(C(J_2), CJ2)
display(eq)
sol_J2 = obs(eq, J_2)
# J2 に対するCソースを得る
for ii in range(len(sol_J3)):
code = sym.ccode(sol_J2[ii], assign_to=('J2'), standard='C89')
print("// Solusion #"+ str(ii) +"\n" + code + "\n")
以上の導出から J1, J2, J3 が求まった.
A3iA2iA1iT6 = A3i*A2i*A1i*T6
for idx in range(12):
display(sym.simplify ( sym.expand( sym.Eq( A3iA2iA1iT6[idx], A456[idx])) ) )
# 上記式の第11式は,
idx=10
eq = sym.simplify ( sym.Eq( A3iA2iA1iT6[idx], A456[idx]))
display(eq)
sol_J5 = obs(eq, J_5)
# J5 に対するCソースを得る
for ii in range(len(sol_J5)):
code = sym.ccode(sol_J5[ii], assign_to=('J5'), standard='C89')
print("// Solusion #"+ str(ii) +"\n" + code + "\n")
# 上記式の第7式は,
idx=6
eq = sym.simplify ( sym.Eq( A3iA2iA1iT6[idx], A456[idx]))
display(eq)
sol_J4 = obs(eq, J_4)
# J4 に対するCソースを得る
for ii in range(len(sol_J4)):
code = sym.ccode(sol_J4[ii], assign_to=('J4'), standard='C89')
print("// Solusion #"+ str(ii) +"\n" + code + "\n")
A5iA4iA3iA2iA1iT6 = A5i*A4i*A3i*A2i*A1i*T6
for idx in range(12):
display(sym.simplify ( sym.expand( sym.Eq( A5iA4iA3iA2iA1iT6[idx], A6[idx])) ) )
# 上記式の第6式は,
idx=5
eq = sym.simplify ( sym.Eq( A5iA4iA3iA2iA1iT6 [idx], A6[idx]))
display(eq)
sol_J6 = obs(eq, J_6)
# J6 に対するCソースを得る
for ii in range(len(sol_J6)):
code = sym.ccode(sol_J6[ii], assign_to=('J6'), standard='C89')
print("// Solusion #"+ str(ii) +"\n" + code + "\n")
以上から J4, J5, J6を得る.